#ifndef TWO_RAD_GRIDDER_HEADER
#define TWO_RAD_GRIDDER_HEADER

#include <vector>
#include <algorithm>
#include "GL\glfw.h"




//CT is the container type
//D is the dimension number
template <typename CT , unsigned int D>
class TwoRadGridder{
public:

	struct GridItemT {

		GridItemT () {
		}

		GridItemT ( const CT & inP) {
			if (&inp != this){
				*this = inp;
			}
		}

		float & operator[]( const unsigned int inNdx ){
			return pos[inNdx];
		}

		const float & operator[](const unsigned int inNdx ) const {
			return pos[inNdx];
		}

		//When assigning external data type to this internal type. (ParticleS to GridItemT)
		GridItemT & operator=( const CT & inP ){
			for (int i = 0; i < 3; ++i){
				pos[i] = inP.pos[i];
				vel[i] = inP.vel[i];
			}

			mass = inP.mass;
			ndx = inP.ndx;
			hashVal = 0;
			return *this;
		}



		unsigned int hashVal;
		glm::vec3 pos;
		float mass;
		glm::vec3 vel;
		unsigned int ndx;
	};

typedef std::pair<unsigned int, unsigned int> GridderCellT;

	TwoRadGridder( const float * minPt, const float * maxPt, float rad)
	{
		unsigned int numCells[2] = {((maxPt[0] - minPt[0])/supportRad)/2, 
			((maxPt[1] - minPt[1])/supportRad)/2};

		InitGridder(minPt, maxPt, numCells);

		sortByHash.mC = this;
	}

	void InitGridder( const float * minPt, const float * maxPt, const unsigned int * numCells){
		
		for (int i = 0; i < D; ++i){
			float range;
			range = (maxPt[i] - minPt[i]);
			mCellSize[i] = range / (float)numCells[i];
			mNumCells[i] = numCells[i];
			mCellSizeRecip[i] = 1.0f / mCellSize[i]; //numCells / range
			mMin[i] = minPt[i];
			mMax[i] = maxPt[i];
		}

		mGrid.resize( mNumCells[0]*mNumCells[1] );
	}

	//Inserts data into the vector and clears the mIsSorted flag
	void InsertData( const CT & inData ) {
		mData.push_back(inData);

		//Sorts the data by the hash.
		SortData();
	}

	void InsertData( const std::vector< CT > & inData ) {
		mData.resize(inData.size());
		std::copy(inData.begin(), inData.end(), mData.begin());

		SortData();
	}

	//Gets a copy of the data
	void CopyData( std::vector< GridItemT > & outData){
		outData.resize( mData.size() );
		std::copy(mData.begin(), mData.end(), outData.begin());
	}

	//Gets a const reference to the data 
	const std::vector< GridItemT > & GetData() {
		return mData;
	}

	//Returns the size of the data
	unsigned int SizeData() {
		return mData.size();
	}

	//Grid block and iterator
	template <class GT>
	class GridBlock {
	public:
		GridBlock () {
			GridderCellT defVal;
			data.resize(4, defVal ); //At least one.
			mSize = 0;
		}

		GridBlock ( TwoRadGridder< GT, D > * inGrid ) {
			GridderCellT defVal;
			data.resize(4, defVal ); //At least one.
			mSize = 0;
			mGrid = inGrid;
		}

		template <class IT>
		class iterator {
		public:
			iterator () {
				mCB = -1;
				mGB = NULL;
				mCurrNdx = -1;
				mData = NULL;

			}

			iterator ( unsigned int ndx, GridBlock * inGB ) {
				mCB = ndx;
				mGB = inGB;
				mData = &inGB->mGrid->mData;

				if (mCB == mGB->mSize){
					mCB = std::max((int)(mCB-1), (int)0);
					mCurrNdx = mGB->data[mCB].second;
					return;
				}
				
				mCurrNdx = mGB->data[mCB].first;
			}

			iterator & operator++ () {
				if (++mCurrNdx == mGB->data[mCB].second){
					//Increment block number
					mCB ++;
					
					//Sets current ndx
					if (mCB >= mGB->mSize){
						mCB = std::max((int)(mCB-1), 0);
						mCurrNdx = mGB->data[mCB].second;
					}
					else if (mCB < 4){
						mCurrNdx = mGB->data[mCB].first;
					}
					
				}

				return *this;
			}

			bool operator==(const iterator & it) const {
				
				return (mCurrNdx == it.mCurrNdx) && (mCB == it.mCB);
			}

			bool operator!=(const iterator & it ) const {

				return (mCurrNdx != it.mCurrNdx) || (mCB != it.mCB);
			}
			

			//operator *
			//operator ->
	
			typename std::vector< GridItemT >::iterator GetData() {
				return mData->begin() + mCurrNdx;
			}

		private:
			unsigned int mCurrNdx;
			unsigned int mCB;
			GridBlock * mGB;
			std::vector< GridItemT > * mData;
		};

		//Return the end iterator
		iterator<GT> end() {
			return iterator<GT>(mSize, this);
		}

		//return the start iterator
		iterator<GT> begin() {
			return iterator<GT>(0, this);
		}

		void Add( GridderCellT & inNew ) {
			//Checks if it is already there.
			
			if (inNew.first == inNew.second){
				return;
			}
			for (int i = 0; i < data.size(); ++i){
				if ((data[i].first == inNew.first &&
					data[i].second == inNew.second) ||
					mSize >= 4){
					return;
				}
			}

			data[mSize++] = inNew;
		}

	private:
		typename TwoRadGridder< GT, D > * mGrid;
		std::vector< GridderCellT > data;
		unsigned int mSize;
	};


	GridBlock<CT> GetNeighborhoodBlock( const CT & inData ){
		
		GridBlock<CT> gb ( this );
		unsigned int cellX, cellY;

		cellX = GetCellNdx(inData[0], 0);
		cellY = GetCellNdx(inData[1], 1);
		
		int incCntr[2] = {0,0};
		incCntr[0] = inData[0] - (cellX * mCellSize[0]) > mCellSize[0]*0.5 ? 1 : -1;
		incCntr[1] = inData[1] - (cellY * mCellSize[1]) > mCellSize[1]*0.5 ? 1 : -1;

		for (int i = 0; i < 2; ++i){
			for (int j = 0; j < 2; ++j){
				cellX = GetCellNdx(inData[0] + (float)mCellSize[0]*(i*incCntr[0]), 0);
				cellY = GetCellNdx(inData[1] + (float)mCellSize[1]*(j*incCntr[1]), 1 );
				
				//hVal is the current cell we want.
				gb.Add(mGrid[ GetHashVal(cellX, cellY) ]);
			}
	}

	return gb;
}

	void Clear() {
		this->mData.clear();
	}

	void Draw(){
		glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
		glBegin(GL_QUADS);
		for (unsigned int i = 0; i < mNumCells[0] ; ++i) {
			for (unsigned int j = 0; j < mNumCells[1] ; ++j) {
				unsigned int cellx, celly;
				cellx = GetCellNdx(mMin[0] +  i * mCellSize[0], 0);
				celly = GetCellNdx(mMin[1] + j * mCellSize[1], 1);
	
				unsigned int hashval = GetHashVal(cellx, celly);
				if (mGrid[hashval].first != mGrid[hashval].second) {

					glVertex2f( mMin[0]+  i * mCellSize[0], mMin[1] + j * mCellSize[1] );
					glVertex2f( mMin[0]+ (i+1)*mCellSize[0],mMin[1] + j*mCellSize[1] );
					glVertex2f( mMin[0] + (i+1) * mCellSize[0], mMin[1] +(j+1) * mCellSize[1] );
					glVertex2f( mMin[0] + (i)*mCellSize[0],mMin[1] + (j+1)*mCellSize[1]  );
				}
			}
		}
		glEnd();
		glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
	}

protected:

	struct _SortFunc {
		bool operator()( const GridItemT & f, const GridItemT & s ) {
			return f.hashVal < s.hashVal;
			//return mC->GetHashVal(f) < mC->GetHashVal(s);
		}

		TwoRadGridder<CT, D> * mC;
	};

	_SortFunc sortByHash;

	friend GridBlock<CT>;
	friend _SortFunc;

	unsigned int GetCellNdx( float val, unsigned int dim ) {
		return std::min(std::max(std::floor( val * mCellSizeRecip[dim]), 0.0f), (float)mNumCells[dim]-1) ;
	}
		
	//XXX Make generic on dimension.
	unsigned int GetHashVal( unsigned int Cellx, unsigned int Celly){
		return Cellx + Celly*mNumCells[0];
	}

	unsigned int GetHashVal( const GridItemT & inVal ){
		return 	GetCellNdx(inVal[0], 0) + GetCellNdx(inVal[1], 1)*mNumCells[0];
	}

	void PrecomputeHash() {
		for (int i = 0; i < mData.size(); ++i){
			mData[i].hashVal = GetHashVal( mData[i]);
		}
	}

	//Sorts the data based on the hash values.
	void SortData() {

		//Precompute Hash values
		PrecomputeHash();

		//First sort the data.
		std::sort(mData.begin(), mData.end(), sortByHash);

		//Go through and set iterators.
		unsigned int firstData = 0;
		unsigned int currCell = 0;
		for ( ; currCell < mGrid.size() && firstData < mData.size(); ++currCell ){
			unsigned int hashVal = GetHashVal( mData[firstData] );
			
			assert(hashVal < mGrid.size());

			//Check if the hash value is greater than the current cell...
			//This means there are empty cells before the current hash value.
			if (currCell < hashVal){
				for(;currCell != hashVal; ++currCell){
					mGrid[currCell].first = firstData;
					mGrid[currCell].second = firstData;
				}
			}

			//We are at the first piece of data with hash value == currCell;
			mGrid[currCell].first = firstData;

			//Iterator through to 1 past the last element with hash value == currCell
			for (;GetHashVal(mData[firstData]) == hashVal;){
				firstData++;				
				if (firstData == mData.size()){
					break;
				}
			}

			mGrid[currCell].second =  firstData;
		}

		//Fill in the rest of the data with the last element.
		for ( ; currCell < mGrid.size(); ++currCell ) {
			mGrid[currCell].first = firstData;
			mGrid[currCell].second = firstData;
		}
	}

	std::vector< GridderCellT > mGrid;



	std::vector< GridItemT > mData;

	//Parameters for the grid's size
	float mCellSize[D];
	unsigned int mNumCells[D];
	float mCellSizeRecip[D];
	float mMin[D];
	float mMax[D];


};

#endif